/**
 *
 */
package jp.seraph.jsmf.codec;

import java.io.BufferedReader;
import java.io.IOException;
import java.io.Reader;
import java.io.Writer;
import java.util.ArrayList;

import org.apache.commons.collections.CollectionUtils;

import net.sf.json.JSONArray;
import net.sf.json.JSONObject;

import jp.seraph.jsade.math.PolynomialFunction;
import jp.seraph.jsade.model.DefaultAngleVelocityCalculator;
import jp.seraph.jsade.model.JointIdentifier;
import jp.seraph.jsade.model.JointIdentifierContainer;
import jp.seraph.jsade.model.PolynomialAngleVelocityCalculator;
import jp.seraph.jsmf.message.MessageUtil;
import jp.seraph.jsmf.motion.DefaultMotion;
import jp.seraph.jsmf.motion.DefaultMotionSession;
import jp.seraph.jsmf.motion.Motion;
import jp.seraph.jsmf.motion.MotionElement;
import jp.seraph.jsmf.motion.MotionSession;
import jp.seraph.jspf.pose.PoseManager;

/**
 *
 *
 */
public class OldMotionCodec implements MotionCodec {
    public OldMotionCodec(PoseManager aPoseManager, JointIdentifierContainer aJointContainer){
        mPoseManager = aPoseManager;
        mJointContainer = aJointContainer;
    }

    private JointIdentifierContainer mJointContainer;
    private PoseManager mPoseManager;

    private static final String CALCULATOR = "Calculator";
    private static final String POLY = "Polynomial";
    private static final String SCALE_FACTOR = "ScaleFactor";
    private static final String ARGUMENT = "Arg";
    private static final String TERMS = "Terms";
    private static final String COEFFICIENT = "Coefficient";
    private static final String EXPOTENT = "Expotent";
    private static final String POSES = "Poses";

    /**
     *
     * @see jp.seraph.jsmf.codec.MotionDecoder#decode(java.io.Reader)
     */
    public Motion decode(Reader aReader) throws IOException {
        Reader tReader = aReader;
        DefaultMotion tResult = new DefaultMotion();

        JSONObject tObject = JSONObject.fromObject(readToEnd(tReader));
        JSONArray tArray = tObject.getJSONArray(POSES);
        JSONObject tCalculatorObject = tObject.getJSONObject(CALCULATOR);

        PolynomialAngleVelocityCalculator tCalculator = decodeCalculator(tCalculatorObject);

        int tSize = tArray.size();
        for (int i = 0; i < tSize; i++) {
            ArrayList<JointIdentifier> tList = new ArrayList<JointIdentifier>();
            CollectionUtils.addAll(tList, mJointContainer.iterator());
            PoseNameLinkMotionElementData tData = new PoseNameLinkMotionElementData(Integer.toString(i), tArray.getString(i), "", 0, tCalculator, tList);
            PoseNameLinkMotionElement tElement = new PoseNameLinkMotionElement(mPoseManager, tData);

            DefaultMotionSession tSession = new DefaultMotionSession(Integer.toString(i));
            tSession.addElement(tElement);
            tResult.addSession(tSession);
        }

        return tResult;
    }

    private PolynomialAngleVelocityCalculator decodeCalculator(JSONObject aObject) {
        PolynomialAngleVelocityCalculator tResult = new DefaultAngleVelocityCalculator();
        tResult.setScaleFactor(aObject.getDouble(SCALE_FACTOR));
        decodeFunctions(tResult, aObject.getJSONArray(POLY));

        return tResult;
    }

    private void decodeFunctions(PolynomialAngleVelocityCalculator aCalculator, JSONArray aArray) {
        int tCount = aArray.size();
        for (int i = 0; i < tCount; i++) {
            aCalculator.setEntry(decodeFunction(aArray.getJSONObject(i).getJSONArray(TERMS)));
        }
    }

    private PolynomialFunction decodeFunction(JSONArray aArray) {
        PolynomialFunction tResult = new PolynomialFunction();

        int tCount = aArray.size();
        for (int i = 0; i < tCount; i++) {
            JSONObject tObject = aArray.getJSONObject(i);
            double tCoefficient = tObject.getDouble(COEFFICIENT);
            double tExponent = tObject.getDouble(EXPOTENT);
            PolynomialFunction.TermEntry tTerm = new PolynomialFunction.TermEntry(tCoefficient, tExponent);
            tResult.addEntry(tTerm);
        }

        return tResult;
    }

    private String readToEnd(Reader aReader) throws IOException {
        BufferedReader tReader = new BufferedReader(aReader);
        StringBuilder tBuilder = new StringBuilder();
        String tLine;
        while ((tLine = tReader.readLine()) != null) {
            tBuilder.append(tLine);
            tBuilder.append(System.getProperty("line.separator"));
        }

        return tBuilder.toString();
    }

    /**
     *
     * @see jp.seraph.jsmf.codec.MotionEncoder#encode(jp.seraph.jsmf.motion.Motion, java.io.Writer)
     */
    public void encode(Motion aMotion, Writer aWriter) throws IOException {
        Motion tMotion = aMotion;
        JSONObject tObject = new JSONObject();
        JSONArray tArray = new JSONArray();
        PoseNameLinkMotionElement tPoseNameElement = null;

        for (MotionSession tSession : tMotion) {
            MotionElement tElement = tSession.toElementList().get(0);
            if(!(tElement instanceof PoseNameLinkMotionElement))
                throw new IllegalArgumentException(MessageUtil.noSupportMotion(tElement.getClass().getName()));

            tPoseNameElement = (PoseNameLinkMotionElement)tElement;
            tArray.add(tPoseNameElement.getPoseName());
        }

        tObject.put(POSES, tArray);
        tObject.put(CALCULATOR, encodeCalculator((PolynomialAngleVelocityCalculator)tPoseNameElement.getCalculator()));

        Writer tWriter = aWriter;
        tWriter.write(tObject.toString());
    }


    private JSONObject encodeCalculator(PolynomialAngleVelocityCalculator aCalculator) {
        JSONObject tResult = new JSONObject();
        tResult.put(SCALE_FACTOR, aCalculator.getScaleFactor());
        tResult.put(POLY, encodeFunctionArray(aCalculator));

        return tResult;
    }

    private JSONArray encodeFunctionArray(PolynomialAngleVelocityCalculator aCalculator) {
        JSONArray tResult = new JSONArray();
        tResult.add(encodeFunction(aCalculator.getFunction()));

        return tResult;
    }

    private JSONObject encodeFunction(PolynomialFunction aFunction) {
        JSONObject tResult = new JSONObject();
        tResult.put(ARGUMENT, 0);
        tResult.put(TERMS, encodeTerms(aFunction));

        return tResult;
    }

    private JSONArray encodeTerms(PolynomialFunction aFunction) {
        JSONArray tResult = new JSONArray();
        for (PolynomialFunction.TermEntry tTerm : aFunction) {
            JSONObject tObject = new JSONObject();
            tObject.put(COEFFICIENT, tTerm.getCoefficient());
            tObject.put(EXPOTENT, tTerm.getExponent());
            tResult.add(tObject);
        }

        return tResult;
    }
}
